/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.situations;

import java.util.Arrays;
import java.util.LinkedList;

import robocup.component.ControllerNumbers;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.actionfactories.BasicActionFactory;
import robocup.component.geometry.Vektor;
import robocup.component.math.RCMath;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.UPDATE_MODE;
import umontreal.iro.lecuyer.probdist.NormalDist;

/**
 * This situation tries to find a scoring kick. It will only be triggered when 
 * we are near the opponents goal with the ball kickable.
 * <br>
 * This situation has the following logic:
 * <ol>
 *  <li>check wether the opponent's goalie will get all scoring attempts (this
 *  is done first as it happens very often and is easy to calculate)</li>
 *  <li>find out what opponents might interfer with our scoring attempt and 
 *  forget about the others</li>
 *  <li>order the left opponents by angle</li>
 *  <li>try to score through each combination of 2 neighbour opponents. if
 *  this is possible determine the through pass most distant to both 
 *  opponents</li>
 *  <li>foreach possible score attempt calcualte the probability that the ball 
 *  enters the goal (see the UvA Trilearn Master thesis for details)</li>
 *  <li>set the best benefit and best action based on the most promising scoring 
 *  attempt</li>
 * </ol>
 * 
 * TODO
 * 
 * - try to outplay the goalie (this will interfer with the 
 * {@link DribbleSituation})
 */
public class ScoreSituation extends Situation {

	// Normal distribution used for scoring probability
	NormalDist normalDist = new NormalDist();

	// the pass evaluator
	PassEvaluator passEvaluator;

	private Vektor GOAL_OTHER;

	private Vektor GOAL_POST_LEFT;

	private Vektor GOAL_POST_RIGHT;

	private Vektor dummyVektor = new Vektor();

  /**
  * if the goalie intercepts our best scoring attempt by more than the
  * {@link #goalieWatchDistance}, he should be watched, as a wrong goalie
  * action could lead to a scoring possibility
  * (=1 : goalie misses the ball by 1m
  * =0 : unsure
  * =-1: goalie gets the ball with an overlap of 1m)
   */
	private double goalieWatchDistance = -5.0;

	// the pass results are store here
	// given 11 opponents there are 11 possibilities for a throughpass
	// +2 as the goalposts are considered as opponents (see simulation method)
	private PassEvaluatorResults[] passResults = PassEvaluatorResults
			.getInstances(13);

	LinkedList<PassEvaluatorResults> tempPassResults = new LinkedList<PassEvaluatorResults>();

	// the offset describes how much time (cycles) an opponent gets
	// for the time he wasn't seen.
	// e.g. an opp. with age=1 might get an offset of 1 as he had the
	// possibility to move in an optimal way during the last cycle
	private int[] playerOffset = new int[11];

	// opponents that should be considered
	private boolean[] relevantOpponents = new boolean[11];

	// opponents inside goalcone (triangle between ball and posts)
	private boolean[] relevantOpponentsInside = new boolean[11];

	// opponents not in goalcone
	private boolean[] relevantOpponentsOutside = new boolean[11];

	// abs. angles of opponents seen from ball
	// +2 for posts
	private double[] opponentAngles = new double[13];

  //Reference to the expected opponents array from the PlayersModel
	private Player[] expectedOpponents = new Player[11];

	// private Vektor[] delayedKickPointsRel = new Vektor[] {
	// new Vektor(0.6, 0.0), new Vektor(0.8, 60.0),
	// new Vektor(0.8, -60.0), new Vektor(0.8, 120.0),
	// new Vektor(0.8, -120) };

	// the goalie is important and should be watched in next cycle
	private boolean shouldWatchGoalie = false;

	private boolean DEBUG = false;

	/**
	 * Constructor - a situation, which simulates different goal-shots, and
	 * checks, if they might be successful or not.
	 * 
	 * @param world -
	 *            the current world-model
	 * @param movements -
	 *            the generic movement model
	 */
	public ScoreSituation(WorldModel world, Movements movements) {

		super(world, movements);

		this.passEvaluator = new PassEvaluator(movements, world.getOpponents());

		this.GOAL_OTHER = SConf.getInstance().GOAL_POS_OTHER.cloned();
		// set posts (with some security)
		this.GOAL_POST_LEFT = new Vektor(Vektor.XY,
				-SConf.getInstance().GOAL_WIDTH / 2,
				SConf.getInstance().half_length);
		this.GOAL_POST_LEFT.x += 0.4;
		this.GOAL_POST_RIGHT = new Vektor(Vektor.XY,
				SConf.getInstance().GOAL_WIDTH / 2,
				SConf.getInstance().half_length);
		this.GOAL_POST_RIGHT.x -= 0.4;
		System.arraycopy(world.getExpectedPlayers(), 11,
				this.expectedOpponents, 0, 11);

	}

	/*
   * (non-Javadoc)
   * @see robocup.component.situations.Situation#update()
	 */
	protected void update() {

		super.update();

		this.reset();
    
		// only update if near enough to opponents goal
		// and no opponent gets both post (used for efficiency)
		if (this.getWorld().getDistance(this.GOAL_OTHER) > 25
				|| this.checkIfOpponentGetsBothSides()
				|| !(this.getWorld().getPlayMode() == PLAY_MODE.PLAY_ON || PLAY_MODE.META_PENALTY_OWN
						.contains(getWorld().getPlayMode()))) {
			return;
		}

    //if the goalie and the goal were not seen we wont try scoring
    //instead we let the player watch in goal direction
		Player oppGoalie = this.getWorld().getOpponentsGoalie();
		if ((oppGoalie == null || (oppGoalie != null && oppGoalie.getAge(this
				.getWorld().getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO) > 5))
				&& (this.getWorld().getAgeOfPoint(
						SConf.getInstance().GOAL_POS_OTHER) > 5
						&& this.getWorld().getAgeOfPoint(
								SConf.getInstance().GOAL_POST_LEFT_OTHER) > 5 && this
						.getWorld().getAgeOfPoint(
								SConf.getInstance().GOAL_POST_RIGHT_OTHER) > 5)) {
			if (DEBUG)
				System.out.println("no goal seen! Returning. "
						+ "(Age Left:"
						+ this.getWorld().getAgeOfPoint(
								SConf.getInstance().GOAL_POST_LEFT_OTHER)
						+ " Right:"
						+ this.getWorld().getAgeOfPoint(
								SConf.getInstance().GOAL_POST_RIGHT_OTHER)
						+ " Center:"
						+ this.getWorld().getAgeOfPoint(
								SConf.getInstance().GOAL_POS_OTHER)
						+ " Goalie:"
						+ ((oppGoalie != null) ? ""
								+ oppGoalie.getAge(this.getWorld()
										.getBodyCycle(),
										UPDATE_MODE.META_BASED_ON_INFO)
								: "null)"));
			if (oppGoalie != null
					&& oppGoalie.getAge(this.getWorld().getBodyCycle(),
							UPDATE_MODE.META_BASED_ON_INFO) > 5
					&& this.getWorld().inOpponentsPenaltyMe())
				this.shouldWatchGoalie = true;

			return;
		}

    //load data
		this.setRelevantOpponents();
		this.setPlayerOffset();
		
    //calcualte best scoring attempt
    this.simulateGoalThroughOpponents(this.getWorld().getBallRef(), this
				.getWorld().getSelfRef());
    
    //set watch goalie flag
		this.setShouldWatchGoalie();
	}

	/**
	 * simulates goal kicks. this method tries to pass between each pair of
	 * neighboured opponents inside the goalcone. (the posts are considered as
	 * non moving opponents in this case)
   * 
   * @param b - the ball
   * @param self - the player
	 */
	private void simulateGoalThroughOpponents(Ball b, Player self) {

		this.setInsidePlayers(b);

		Arrays.fill(this.opponentAngles, Double.MAX_VALUE);
		this.tempPassResults.clear();

		// set angles
		this.opponentAngles[0] = b.getAngleTo(this.GOAL_POST_LEFT);

		int angleIndex = 1;
		for (int i = 0; i < this.expectedOpponents.length; i++) {
			if (this.relevantOpponentsInside[i]) {
				this.opponentAngles[angleIndex++] = b
						.getAngleTo(this.expectedOpponents[i]);
			}
		}

		this.opponentAngles[angleIndex] = b.getAngleTo(this.GOAL_POST_RIGHT);

		// order angles (angle to left post should be first, angle to right post
		// last one in array)
		Arrays.sort(this.opponentAngles);

		// abs. angle to left opponent|post
		double leftAngleMax;
		// abs. angle to rightopponent|post
		double rightAngleMax;
		// (left + right) / 2
		double avgAngle;

		Vektor scorePoint = new Vektor();
		double scoreBenefit = 0;

		angleIndex = 0;
    
    //try a through pass for each combination of neighbour opponents in the 
    //triangle of ball to goal posts
		while (angleIndex < this.opponentAngles.length - 1
				&& this.opponentAngles[angleIndex + 1] != Double.MAX_VALUE) {
			PassEvaluatorResults results = this.passResults[angleIndex];

      //load values for PassEvaluator
			leftAngleMax = this.opponentAngles[angleIndex];
			rightAngleMax = this.opponentAngles[++angleIndex];
			avgAngle = (leftAngleMax + rightAngleMax) / 2.0;
			scorePoint.copy(this.getScorePoint(b, avgAngle));

			// simulate passing to score point
			this.passEvaluator.setParams(self, b, scorePoint,
					this.playerOffset, this.expectedOpponents,
					this.relevantOpponents, avgAngle - leftAngleMax,
					rightAngleMax - avgAngle);

			this.passEvaluator.evaluate(results);

			// remember results
			this.tempPassResults.add(results);

			// passing possible
			if (!results.isNoPassing()) {
				double deviation = results.getAvgAngle();
				// adjust score point by deviation
				scorePoint.copy(this.getScorePoint(b, scorePoint, deviation));
				// get score probability
				scoreBenefit = this.getScoreProb(self, b, scorePoint);
				if (scoreBenefit > this.benefit) {
					this.benefit = scoreBenefit;
					this.bestAction = BasicActionFactory.kickFastest(self, b,
							scorePoint);
				}
				// can score.... -> kick it
				if (this.benefit > 0.85) {
					if (this.DEBUG)
						System.out
								.println("ScoreSituation: Can score!!! ScorePoint:"
										+ scorePoint
										+ " Prob:"
										+ this.benefit
										+ " Action:" + this.bestAction);
					return;
				}
			} else if (this.DEBUG)
				System.out.println("Scoring Blocked:"
						+ Arrays.toString(results.getOpponentAngleLeft()) + " "
						+ Arrays.toString(results.getOpponentAngleRight()));

		}
	}

	/**
	 * calculates the score point given the ball the tested score point and a
	 * deviation from that point.
	 * 
	 * @param ball -
	 *            ball
	 * @param testedScorePoint -
	 *            old score point
	 * @param deviation -
	 *            deviation as angle from old score point
	 * @return - point inside opponents goal where ball should hit
	 */
	private Vektor getScorePoint(Ball ball, Vektor testedScorePoint,
			double deviation) {

		double shootAngle = Vektor.normalize(ball.getAngleTo(testedScorePoint)
				+ deviation);
		return this.getScorePoint(ball, shootAngle);
	}

	/**
	 * calculates the score point given the ball and the abs. shoot angle.
	 * 
	 * @param ball -
	 *            ball
	 * @param shootAngle -
	 *            abs angle from ball to score point
	 * @return - point inside opponents goal where ball should hit
	 * 
	 * @exception RuntimeException -
	 *                if score point outside goal
	 */
	private Vektor getScorePoint(Ball ball, double shootAngle) {

		this.dummyVektor.y = SConf.getInstance().half_length;
		this.dummyVektor.x = (this.dummyVektor.y - ball.getPosition().y)
				* Math.tan(Math.toRadians(shootAngle)) + ball.getPosition().x;
		if (Math.abs(this.dummyVektor.x) > SConf.getInstance().GOAL_WIDTH / 2.0)
			throw new RuntimeException(this.getWorld()
					.getCycleAndPlayerString()
					+ " ScoreSituation not hitting goal. X-Koord:"
					+ this.dummyVektor.x + " " + ball);

		return this.dummyVektor;

	}

	/**
	 * decides wether the goalie is important enough to be watched in next
	 * cycle. the goalie is important if he is the opponent preventing us from
	 * scoring. the goalie is less important if we don't have a chance to score
	 * against him. In case it's more important to watch out for teammates to pass
	 * to.
	 */
	private void setShouldWatchGoalie() {

		Player goalie = this.getWorld().getOpponentsGoalie();
		if (goalie != null
				&& this.relevantOpponentsInside[goalie.getNumber() - 1]
				&& (this.tempPassResults.get(0).getMinDistanceRight() > this.goalieWatchDistance
						- this.getWorld().getAge(goalie) || this.tempPassResults
						.getLast().getMinDistanceLeft() > this.goalieWatchDistance
						- this.getWorld().getAge(goalie))) {
			if (this.DEBUG)
				System.out.println(this.getWorld().getCycleAndPlayerString()
						+ " SHOULD WATCH GOALIE!!");
			this.shouldWatchGoalie = true;
		} else {
			this.shouldWatchGoalie = false;
			if (this.DEBUG) {
				if (goalie == null) {
					System.out.println(" NOT SHOULD WATCH GOALIE!! NO GOALIE");
				} else {
					System.out
							.println(this.getWorld().getCycleAndPlayerString()
									+ " NOT SHOULD WATCH GOALIE!! "
									+ "Age:"
									+ this.getWorld().getAge(goalie)
									+ " O0:"
									+ this.tempPassResults.get(0)
											.getMinDistanceRight()
									+ " O1:"
									+ this.tempPassResults.getLast()
											.getMinDistanceLeft()
									+ this.relevantOpponentsInside[goalie
											.getNumber() - 1]);
				}
			}
		}
	}

	/**
	 * set the opponents that are considered in the simulation
	 */
	private void setRelevantOpponents() {

		Arrays.fill(this.relevantOpponents, true);
		// filtering distant players (simple version)
		for (int playerIndex = 0; playerIndex < 11; playerIndex++) {
			if (!this.expectedOpponents[playerIndex].isReliable()
					|| this.expectedOpponents[playerIndex].getAge(this
							.getWorld().getBodyCycle(),
							UPDATE_MODE.META_BASED_ON_INFO) > 10) {
				this.relevantOpponents[playerIndex] = false;
			}
		}
		Arrays.fill(this.relevantOpponentsInside, false);
		Arrays.fill(this.relevantOpponentsOutside, false);
	}

	/**
	 * players that were not seen might be nearer than expected. own players
	 * need one cycle to react.
	 * 
	 */
	private void setPlayerOffset() {

		int j = 0;
		for (int playerIndex = 0; playerIndex < 11; playerIndex++) {
			j = 0;
			if (this.expectedOpponents[playerIndex].getAge(this.getWorld()
					.getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO) >= 2) {
				j += 2;
			} else if (this.expectedOpponents[playerIndex].getAge(this
					.getWorld().getBodyCycle(), UPDATE_MODE.META_BASED_ON_INFO) == 1) {
				j += 1;
			}
			j -= this.expectedOpponents[playerIndex].getTackledeadCountdown();
			this.playerOffset[playerIndex] = j;

		}
	}

	/**
	 * sets the player inside the goal cone. this method should only be used
	 * after simulating kicks to the left and right post as ist uses results of
	 * this simulations.
	 */
	public void setInsidePlayers(Ball b) {

		Arrays.fill(this.relevantOpponentsInside, false);
		Arrays.fill(this.relevantOpponentsOutside, false);

		double angleToLeftPost = b.getAngleTo(this.GOAL_POST_LEFT);
		double angleToRightPost = b.getAngleTo(this.GOAL_POST_RIGHT);
		double angleToOpp;

		for (int i = 0; i < this.expectedOpponents.length; i++) {
			if (this.relevantOpponents[i]) {
				angleToOpp = b.getAngleTo(this.expectedOpponents[i]);
				if (angleToLeftPost < angleToOpp
						&& angleToOpp < angleToRightPost) {
					this.relevantOpponentsInside[i] = true;
				} else {
					this.relevantOpponentsOutside[i] = true;
				}
			}
		}
	}

	/**
	 * it happens often that the goalie is able to defend both sides of the goal
	 * this method checks if that occurs. it is used for efficiency as other 
   * players don't have to be considered in this case. 
	 * 
	 * @return true -> true, false -> unsure
	 */
	private boolean checkIfOpponentGetsBothSides() {

		double distBallToLeftPost = this.getWorld().getBallRef().getDistance(
				this.GOAL_POST_LEFT);
		double distBallToRightPost = this.getWorld().getBallRef().getDistance(
				this.GOAL_POST_RIGHT);

		int timeBallToLeftPost = RCMath.getCyclesForBallReachingDistance(SConf
				.getInstance().BALL_SPEED_MAX, distBallToLeftPost);
		int timeBallToRightPost = RCMath.getCyclesForBallReachingDistance(SConf
				.getInstance().BALL_SPEED_MAX, distBallToRightPost);

		if (timeBallToLeftPost == Integer.MAX_VALUE
				|| timeBallToRightPost == Integer.MAX_VALUE) {
			return false;
		}

		for (Player opp : this.expectedOpponents) {
			if (!opp.isReliable())
				continue;
			else {
				double secureDistance = opp.isGoalie() ? opp.getCatchDistance()
						: opp.getKickDistance();

				// even if the opponent has to turn 3x he would get the balls
				// kicked to the posts
				if (this.getMovements().reaches(opp.getNumber() + 11 - 1,
						timeBallToLeftPost, this.GOAL_POST_LEFT,
						opp.getPosition(), opp.getBodyDir(), 60.0, 150.0,
						secureDistance) != ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT
						&& this.getMovements().reaches(
								opp.getNumber() + 11 - 1, timeBallToRightPost,
								this.GOAL_POST_RIGHT, opp.getPosition(),
								opp.getBodyDir(), 60.0, 150.0, secureDistance) != ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT) {
					if (this.DEBUG)
						System.out.println(this.getWorld()
								.getCycleAndPlayerString()
								+ " OPPonent gets both sides" + opp);
					return true;
				}
			}
		}
		return false;
	}

	/**
	 * The probability to score a goal. based on Uva Trilearn MT. P.140
	 * The probability depends on the angle to the score point and the distance
   * to that point.
   * 
	 * @param p - the Player
	 * @param b - the Ball
	 * @param scorePos - the point on the goal line to score
   * 
	 * @return scoring probability (0..1)
	 */
	private double getScoreProb(Player p, Ball b, Vektor scorePos) {

		// TODO copied that version from the pass situation
		// should be adapted and made more efficient (rw)

		// see MT P.140
		double hit_x = scorePos.getX();
		double distUpperPost = Math.abs(SConf.getInstance().GOAL_WIDTH) / 2
				+ hit_x;

		double hitAngle = Vektor.normalize(90.0 - b.getAngleTo(scorePos));

		// switch sides using symmetry
		if (hitAngle < 0) {
			hitAngle *= -1;
			distUpperPost = SConf.getInstance().GOAL_WIDTH - distUpperPost;
		}

		double distanceToScorePoint = Math
				.abs((SConf.getInstance().half_length - b.getPosition().y)
						* Math.sin(hitAngle));

		double correctedDistUpperPost = Math.sin(Math.toRadians(hitAngle))
				* distUpperPost;
		double correctedDistLowerPost = Math.sin(Math.toRadians(hitAngle))
				* (SConf.getInstance().GOAL_WIDTH - distUpperPost);

		double sigma = -1.8 * Math.log(1 - distanceToScorePoint / 45.0)
				* b.getSpeed().getLength();
		if (sigma <= 0.0)
			return 0.0;

		// set normal distribution values
		this.normalDist.setParams(0.0, sigma);
		double p_out_up = this.normalDist.barF(correctedDistLowerPost);
		double p_out_down = this.normalDist.barF(correctedDistUpperPost);
    
    //debug output
		if (this.DEBUG) {
			System.out.println("SCOREPROB: HIT_X:" + hit_x);
			System.out.println(" Sigma:" + sigma + " Angle:" + hitAngle
					+ " cUp:" + correctedDistUpperPost + " cLow:"
					+ correctedDistLowerPost);
			System.out.println(" Out_u:" + p_out_up + " Out_d:" + p_out_down);
			System.out.println("SCOREPROB:" + (1 - p_out_down - p_out_up));
		}
		return Math.pow(1 - p_out_down - p_out_up, 6);
	}

	/**
	 * reset values
	 * 
	 */
	private void reset() {

		this.setBenefit(0.0);
		this.setBestAction(null);
		this.shouldWatchGoalie = false;
	}

	/**
	 * @return Returns the shouldWatchGoalie.
	 */
	public boolean isShouldWatchGoalie() {

		if (!this.wasUpdated())
			this.update();

		return this.shouldWatchGoalie;
	}

	/**
	 * the ball resulting from the score attempt.
	 * 
	 * @return the Ball as it will result from the best action of this situation
   * set back to this cycle.
	 */
	public Ball getResultingBall() {

		if (!this.wasUpdated()) {
			this.update();
		}
		if (this.bestAction == null) {
			return null;
		}

		Ball ball = this.getWorld().getBallRef().cloned();
		ball.predictThisAfterAction(this.getWorld().getSelfRef(),
				this.bestAction);

		// ball contains the next ball -> set it back 1 cycle
		ball.getPosition().copy(this.getWorld().getBallRef().getPosition());
		ball.getSpeed().mult(1.0 / SConf.getInstance().BALL_DECAY);

		return ball;
	}
}
